#include "lane_full_matching/lfm.h"
#include "sign_pole_matching/spm.h"
#include "tracking/main_tracker.h"
#include "output/output.h"
#include "output/output_impl.h"
#include "vectorization/sign_vec.h"
#include "base/defines.h"
#include "base/fileUtils.h"
#include "base/utility.h"
#include "base/log.h"
#include "config.h"
#include "io.h"

#include <iostream>
#include <fstream>
#include <memory>

int main()
{
    Config cfg;
    cfg.ReadConfigFile("../config/andres_config.json");
    Log::ROOT = "./log";

    Camera camera;
    camera.LoadFromCfg(cfg.cam_cfg_file);

    GpsBuffer gps_buffer;
    HDMapBuffer hdmap_buffer;
    DlFrameBuffer dlframe_buffer(cfg.deep_learning_lane_path, cfg.deep_learning_sign_path);
    gps_buffer.LoadDataFromCsv(cfg.gps_file_);

    Filtering::ParticleFilter pf;
    double sigma_init[3] = {5.0, 5.0, 0.01}; 
    double sigma_lane_fixed[3] = {5.0, 0.5, 0.01}; 
    double sigma_sign_pole_fixed[3] = {0.5, 0.5, 0.01}; 
    double sigma_no_fix[3] = {5.0, 5.0, 0.01}; 

    TRACK::MainTracker tracker(cfg.img_path_);

    GeoPoint gps_point, prev_point;
    std::map<std::string, Pose> poseMap;
    while(gps_buffer.ReadNext(gps_point))
    {
        DlFrame dlFrame;
        if(!dlframe_buffer.ReadNext(gps_point, camera, dlFrame))
        {
            prev_point = gps_point;
            LOG("can not read segmentation file ... ") << gps_point.imagename << std::endl;
            continue;
        }

        hdmap_frame hdmap_f = hdmap_buffer.LoadMapDataByPostion(gps_point, 100.0);
        if(hdmap_f.hdmap_lanes.empty())
        {
            prev_point = gps_point;
            LOG("can not read hdmap data ... ") << gps_point.imagename << std::endl;
            continue;
        }

        if(!pf.initialized())
        {
            pf.init(gps_point.x, gps_point.y, gps_point.z, gps_point.bearing, sigma_init, 3000);
        }
        else
        {
            double sigma_predict[3] = {0.3, 0.3, 0.01}; 
            double delta_t = (gps_point.timestamp - prev_point.timestamp) / 1000.0;
			pf.prediction(delta_t, sigma_predict, prev_point.speed, prev_point.yawrate);
            pf.GetCurLoc(gps_point);
		}

        LOG("start process ... ") << gps_point.imagename << std::endl;
        std::shared_ptr<DbgFrame> dbg_frame = std::make_shared<DbgFrameImpl>(cfg.img_path_, gps_point, &camera);
        dbg_frame->DrawDlFrame2D(dlFrame, cv::Scalar(0, 0, 255));

        Pose pose;
        double rmse = 0.0;
        LFM::LaneMatching lfm;
        bool is_fixed = false;
        if(lfm.Calculate_Pose(dlFrame, hdmap_f.hdmap_lanes, gps_point, camera, pose, rmse, dbg_frame.get()))
        {           
            gps_point.x = pose.position.x();
            gps_point.y = pose.position.y();
            gps_point.z = pose.position.z();

            Eigen::Matrix3d R = pose.orientation.toRotationMatrix();
            Eigen::Vector3d forward = R *  Eigen::Vector3d(1.0, 0.0, 0.0);
            gps_point.bearing = std::atan2(forward.y(), forward.x());

            pf.updateWeights(gps_point.x, gps_point.y, gps_point.z, gps_point.bearing, sigma_lane_fixed);
            is_fixed = true;
        }
        else    
        {
           pf.updateWeights(gps_point.x, gps_point.y, gps_point.z, gps_point.bearing, sigma_no_fix);
        }

        double spm_rmse = 0.0;
        SPM::SignPoleMatching spm;
        if (spm.Calculate_Pose(dlFrame, hdmap_f.hdmap_signs, pose, camera, spm_rmse, dbg_frame.get()))
        {
            gps_point.x = pose.position.x();
            gps_point.y = pose.position.y();
            gps_point.z = pose.position.z();

            Eigen::Matrix3d R = pose.orientation.toRotationMatrix();
            Eigen::Vector3d forward = R * Eigen::Vector3d(1.0, 0.0, 0.0);
            gps_point.bearing = std::atan2(forward.y(), forward.x());
            pf.updateWeights(gps_point.x, gps_point.y, gps_point.z, gps_point.bearing, sigma_sign_pole_fixed);
            is_fixed = true;
        }

        pf.resample();
        pf.Print(dbg_frame.get());
        pf.GetCurrentLoc(gps_point.x, gps_point.y, gps_point.z, gps_point.bearing);
        prev_point = gps_point;

        //Reconstruction
        tracker.Tracking(dlFrame, hdmap_f, gps_point, camera, pose, dbg_frame.get());     
        if(is_fixed)
        {
            poseMap[gps_point.imagename] = pose;
        }
        SignBatch to_Vec;
        ville3d::SignVec signVec(cfg.img_path_);
        while(tracker.TakeFirst(to_Vec))
        {
            signVec.DoWork(to_Vec, poseMap, camera);
        }
        
        dbg_frame->DrawHDmapProjectView(hdmap_f, pose, cv::Scalar(255, 255, 255));
        dbg_frame->DrawHorizontial(gps_point, hdmap_f.hdmap_lanes);
        dbg_frame->PrintIntoImg("./dump/" + gps_point.imagename);
        LOG("end process ... ") << gps_point.imagename << std::endl;
    }
    return 0;
}